#include "rt_scheduler.h"
#include "rt_thread.h"
#include "rt_hw.h"

rt_list_t rt_thread_priority_table[RT_THREAD_PRIORITY_MAX]; 
/* 线程就绪优先级组 */
rt_uint32_t rt_thread_ready_priority_group;
rt_uint32_t rt_current_priority; 

extern struct rt_thread rt_flag1_thread;
extern struct rt_thread rt_flag2_thread;
extern struct rt_thread idle;
struct rt_thread *rt_current_thread;
/* 中断计数器 */
volatile rt_uint8_t rt_interrupt_nest;  
static rt_tick_t rt_tick = 0; /* 系统时基计数器 */   



/*
* __lowest_bit_bitmap[] 数组的解析
* 将一个8位整形数的取值范围0~255作为数组的索引，索引值第一个出现1(从最低位开始)的位号作为该数组索引下的成员值。
* 举例：十进制数10的二进制为：0000 1010,从最低位开始，第一个出现1的位号为bit1，则有__lowest_bit_bitmap[10]=1
* 注意：只需要找到第一个出现1的位号即可
*/
const rt_uint8_t __lowest_bit_bitmap[] =
{
    /* 00 */ 0, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
    /* 10 */ 4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
    /* 20 */ 5, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
    /* 30 */ 4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
    /* 40 */ 6, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
    /* 50 */ 4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
    /* 60 */ 5, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
    /* 70 */ 4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
    /* 80 */ 7, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
    /* 90 */ 4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
    /* A0 */ 5, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
    /* B0 */ 4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
    /* C0 */ 6, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
    /* D0 */ 4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
    /* E0 */ 5, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
    /* F0 */ 4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0
};


/**
* 该函数用于从一个32位的数中寻找第一个被置1的位（从低位开始），
* 然后返回该位的索引（即位号）
*
* @return 返回第一个置1位的索引号。如果全为0，则返回0。
*/
int __rt_ffs(int value)
{
    /* 如果值为0，则直接返回0 */
    if (value == 0) return 0;     

    /* 检查 bits [07:00]
    这里加1的原因是避免当第一个置1的位是位0时
    返回的索引号与值都为0时返回的索引号重复 */
    if (value & 0xff)                       
        return __lowest_bit_bitmap[value & 0xff] + 1;

    /* 检查 bits [15:08] */
    if (value & 0xff00)       
        return __lowest_bit_bitmap[(value & 0xff00) >> 8] + 9;

    /* 检查 bits [23:16] */
    if (value & 0xff0000)         
        return __lowest_bit_bitmap[(value & 0xff0000) >> 16] + 17;

    /* 检查 bits [31:24] */          
    return __lowest_bit_bitmap[(value & 0xff000000) >> 24] + 25;
}















void rt_system_scheduler_init(void)
{
    register rt_base_t offset;

    /* 线程就绪列表初始化 */
    for (offset = 0; offset < RT_THREAD_PRIORITY_MAX; offset ++)
    {
            rt_list_init(&rt_thread_priority_table[offset]);
    }

    /* 初始化当前优先级为空闲线程的优先级 */
    rt_current_priority = RT_THREAD_PRIORITY_MAX - 1;  

    /* 初始化当前线程控制块指针 */
    rt_current_thread = RT_NULL;

    /* 初始化线程就绪优先级组 */
    rt_thread_ready_priority_group = 0; 
}

/* 启动系统调度器 */
void rt_system_scheduler_start(void)
{
    register struct rt_thread *to_thread;
    register rt_ubase_t highest_ready_priority;

    /* 获取就绪的最高优先级 */                                 
    highest_ready_priority = __rt_ffs(rt_thread_ready_priority_group) - 1;

    /* 获取将要运行线程的线程控制块 */                     
    to_thread = rt_list_entry(rt_thread_priority_table[highest_ready_priority].next,
                            struct rt_thread,
                            tlist);

    rt_current_thread = to_thread;                             
    /* 切换到新的线程 */
    rt_hw_context_switch_to((rt_uint32_t)&to_thread->sp);     
    /* 永远不会返回 */     
}

void rt_thread_delay(rt_tick_t tick)
{
    register rt_base_t temp;
    struct rt_thread *thread;

    /* 失能中断 */
    temp = rt_hw_interrupt_disable();

    thread = rt_current_thread;
    thread->remaining_tick = tick;

    /* 改变线程状态 */
    thread->stat = RT_THREAD_SUSPEND;               
    rt_thread_ready_priority_group &= ~thread->number_mask;  

    /* 使能中断 */
    rt_hw_interrupt_enable(temp);

    /* 进行系统调度 */
    rt_schedule();
}


/* 系统调度 */
void rt_schedule(void)
{
    rt_base_t level;
    register rt_ubase_t highest_ready_priority;
    struct rt_thread *to_thread;
    struct rt_thread *from_thread;

    /* 关中断 */
    level = rt_hw_interrupt_disable();

    /* 获取就绪的最高优先级 */                        
    highest_ready_priority = __rt_ffs(rt_thread_ready_priority_group) - 1;
    /* 获取就绪的最高优先级对应的线程控制块 */             
    to_thread = rt_list_entry(rt_thread_priority_table[highest_ready_priority].next,
                                struct rt_thread,
                                tlist);

    /* 如果目标线程不是当前线程，则要进行线程切换 */
    if (to_thread != rt_current_thread)             
    {
        rt_current_priority = (rt_uint8_t)highest_ready_priority;
        from_thread         = rt_current_thread;
        rt_current_thread   = to_thread;

        rt_hw_context_switch((rt_uint32_t)&from_thread->sp,
                            (rt_uint32_t)&to_thread->sp);


    }

    /* 开中断 */
    rt_hw_interrupt_enable(level);
    /* 产生上下文切换 */
    rt_hw_context_switch((rt_uint32_t)&from_thread->sp,(rt_uint32_t)&to_thread->sp);
}


#define IDLE_THREAD_STACK_SIZE      512

ALIGN(RT_ALIGN_SIZE)
static rt_uint8_t rt_thread_stack[IDLE_THREAD_STACK_SIZE];

/* 空闲线程的线程控制块 */
struct rt_thread idle;
rt_ubase_t  rt_idletask_ctr = 0;

static void rt_thread_idle_entry(void *parameter)
{
    parameter = parameter;
    while (1)
    {
        rt_idletask_ctr ++;
    }
}

void rt_thread_idle_init(void)
{

    /* 初始化线程 */     
    rt_thread_init(&idle,
                // "idle",
                rt_thread_idle_entry,
                RT_NULL,
                &rt_thread_stack[0],
                sizeof(rt_thread_stack),
                RT_THREAD_PRIORITY_MAX-1);

    /* 将线程插入到就绪列表 */     
    // rt_list_insert_before( &(rt_thread_priority_table[RT_THREAD_PRIORITY_MAX-1]),&(idle.tlist) );
    rt_thread_startup(&idle);
}

void rt_tick_increase(void)
{
    rt_ubase_t i;
    struct rt_thread *thread;
    rt_tick ++;      
    /* 扫描就绪列表中所有线程的remaining_tick，如果不为0，则减1 */
    for(i=0; i<RT_THREAD_PRIORITY_MAX; i++)   
    {
        thread = rt_list_entry( rt_thread_priority_table[i].next,
                                struct rt_thread,
                                tlist);
        if(thread->remaining_tick > 0)
        {
            thread->remaining_tick --;
            if(thread->remaining_tick == 0)
            {
                //rt_schedule_insert_thread(thread);
                rt_thread_ready_priority_group |= thread->number_mask; 
            }
        }
    }

    /* 系统调度 */
    rt_schedule();          
}

void rt_interrupt_enter(void) 
{
    rt_base_t level;

    /* 关中断 */
    level = rt_hw_interrupt_disable();

    /* 中断计数器++ */
    rt_interrupt_nest ++;

    /* 开中断 */
    rt_hw_interrupt_enable(level);
}


void rt_interrupt_leave(void)
{
    rt_base_t level;

    /* 关中断 */
    level = rt_hw_interrupt_disable();

    /* 中断计数器-- */
    rt_interrupt_nest --;

    /* 开中断 */
    rt_hw_interrupt_enable(level);
}


void rt_schedule_insert_thread(struct rt_thread *thread)
{
    register rt_base_t temp;

    /* 关中断 */
    temp = rt_hw_interrupt_disable();

    /* 改变线程状态 */
    thread->stat = RT_THREAD_READY;

    /* 将线程插入就绪列表 */
    rt_list_insert_before(&(rt_thread_priority_table[thread->current_priority]),
                        &(thread->tlist));

    /* 设置线程就绪优先级组中对应的位 */
    rt_thread_ready_priority_group |= thread->number_mask;

    /* 开中断 */
    rt_hw_interrupt_enable(temp);
}

void rt_schedule_remove_thread(struct rt_thread *thread)
{
    register rt_base_t temp;


    /* 关中断 */
    temp = rt_hw_interrupt_disable();

    /* 将线程从就绪列表删除 */
    rt_list_remove(&(thread->tlist));

    if (rt_list_isempty(&(rt_thread_priority_table[thread->current_priority])))
    {
        rt_thread_ready_priority_group &= ~thread->number_mask;
    }

    /* 开中断 */
    rt_hw_interrupt_enable(temp);
}

rt_thread_t rt_thread_self(void)
{
    return rt_current_thread;
}